/**************************************************************************************************
* Title: SumMatrix.cpp
* Author: Gael Huber
* Description: Contains various function definition of 4x4 Matrix functions
**************************************************************************************************/
#include "SumMath.h"

/**************************************************************************************************
* Returns product of two matrices
**************************************************************************************************/
Matrix& Matrix::operator*=(const Matrix& rhs) {
	// Copy this matrix over
	Matrix lhs = *this;
	
	// Populate each row and column of the new matrix
	for(int i = 0; i < 4; ++i) {
		for(int j = 0; j < 4; ++j) {
			// Assign dot product
			m[i][j] = lhs(i, 0)*rhs(0, j) + lhs(i, 1)*rhs(1, j) + lhs(i, 2)*rhs(2, j) + lhs(i, 3)*rhs(3, j);
		}
	}
	return *this;
}

/**************************************************************************************************
* Returns product of two matrices
**************************************************************************************************/
Matrix Matrix::operator*(const Matrix& rhs) const {
	// Copy this matrix over
	Matrix prod;
	
	// Populate each row and column of the new matrix
	for(int i = 0; i < 4; ++i) {
		for(int j = 0; j < 4; ++j) {
			// Assign dot product
			prod(i, j) = m[i][0]*rhs(0, j) + m[i][1]*rhs(1, j) + m[i][2]*rhs(2, j) + m[i][3]*rhs(3, j);
		}
	}

	return prod;
}

/**************************************************************************************************
* Calculate the determinant of the matrix
**************************************************************************************************/
float MatrixDeterminant(const Matrix* pM) {
	return	(pM->_33*pM->_44 - pM->_34*pM->_43)*(pM->_11*pM->_22 - pM->_12*pM->_21)
		+	(pM->_32*pM->_44 - pM->_34*pM->_41)*(pM->_13*pM->_21 - pM->_11*pM->_23)
		+	(pM->_32*pM->_43 - pM->_33*pM->_42)*(pM->_11*pM->_24 - pM->_14*pM->_21)
		+	(pM->_31*pM->_44 - pM->_34*pM->_41)*(pM->_12*pM->_23 - pM->_13*pM->_22)
		+	(pM->_31*pM->_43 - pM->_33*pM->_41)*(pM->_14*pM->_22 - pM->_12*pM->_24)
		+	(pM->_31*pM->_42 - pM->_32*pM->_41)*(pM->_13*pM->_24 - pM->_14*pM->_23);
}

/**************************************************************************************************
* Decompose
**************************************************************************************************/
int MatrixDecompose(Vector3* pOutScale, Quaternion* pOutRotation, Vector3* pOutTranslation, 
	const Matrix* pM) {
	// Create row vectors, u, v, and w
	Vector3 u(&pM->_11);
	Vector3 v(&pM->_21);
	Vector3 w(&pM->_31);

	// Generate three orthonormal vectors u*, v* and w* (we will simultaneously generate the scaling factors)
	float s1(Vec3Length(&u));
	if(s1 == 0.0f)	return 0;
	Vector3 uO(u * (1.0f / s1));
	Vector3 temp(v - (Vec3Dot(&v, &uO)*uO));
	float s2(Vec3Length(&temp));
	if(s2 == 0.0f)	return 0;
	Vector3 vO(temp * (1.0f / s2));
	temp = (w - (Vec3Dot(&w, &uO)*uO) - (Vec3Dot(&w, &vO)*vO));
	float s3(Vec3Length(&temp));
	if(s3 == 0.0f)	return 0;
	Vector3 wO(temp * (1.0f / s3));

	Matrix R(
		uO.x, uO.y, uO.z, 0.0f,
		vO.x, vO.y, vO.z, 0.0f,
		wO.x, wO.y, wO.z, 0.0f,
		0.0f, 0.0f, 0.0f, 1.0f);
	float det(MatrixDeterminant(&R));
	if(det == -1.0f) {
		R._31 *= -1.0f;
		R._32 *= -1.0f;
		R._33 *= -1.0f;
		s3 *= -1.0f;
	}

	// Populate scale
	pOutScale->x = s1;
	pOutScale->y = s2;
	pOutScale->z = s3;

	// Populate quaternion
	QuaternionRotationMatrix(pOutRotation, &R);

	// Populate translation
	pOutTranslation->x = pM->_41;
	pOutTranslation->y = pM->_42;
	pOutTranslation->z = pM->_43;

	return 1;
}

/**************************************************************************************************
* Transpose the matrix
**************************************************************************************************/
Matrix* MatrixTranspose(Matrix* pOut, const Matrix* pM) {
	pOut->_11 = pM->_11;	pOut->_12 = pM->_21;	pOut->_13 = pM->_31;	pOut->_14 = pM->_41;
	pOut->_21 = pM->_12;	pOut->_22 = pM->_22;	pOut->_23 = pM->_32;	pOut->_24 = pM->_42;
	pOut->_31 = pM->_13;	pOut->_32 = pM->_23;	pOut->_33 = pM->_33;	pOut->_34 = pM->_43;
	pOut->_41 = pM->_14;	pOut->_42 = pM->_24;	pOut->_43 = pM->_34;	pOut->_44 = pM->_44;
	return pOut;
}

/**************************************************************************************************
* Product of two matrices
**************************************************************************************************/
Matrix* MatrixMultiply(Matrix* pOut, const Matrix* pM1, const Matrix* pM2) {
	// Populate each row and column of the new matrix
	for(int i = 0; i < 4; ++i) {
		for(int j = 0; j < 4; ++j) {
			// Assign dot product
			pOut->m[i][j] = pM1->m[i][0]*pM2->m[0][j] + pM1->m[i][1]*pM2->m[1][j] + pM1->m[i][2]*pM2->m[2][j] + pM1->m[i][3]*pM2->m[3][j];
		}
	}
	return pOut;
}

/**************************************************************************************************
* Multiply two matrices and then return the transposition
**************************************************************************************************/
Matrix* MatrixMultiplyTranspose(Matrix* pOut, const Matrix* pM1, const Matrix* pM2) {
	Matrix m;
	MatrixMultiply(&m, pM1, pM2);
	MatrixTranspose(pOut, &m);
	return pOut;
}

/**************************************************************************************************
* Return the inverse of the matrix as well as the determinant if the answer is non-null
**************************************************************************************************/
Matrix* MatrixInverse(Matrix* pOut, float* pDeterminant, const Matrix* pM) {
	// First calculate the determinant
	float determinant(MatrixDeterminant(pM));

	// Only proceed if determinant is non-zero
	if(determinant != 0.0f) {
		// Set the inverse matrix to start as the identity matrix (acts as right-hand portion of augmented matrix)
		MatrixIdentity(pOut);
		Matrix base(*pM);
		
		// Iterate through all pivots
		for(int pivot = 0; pivot < 4; ++pivot) {
			// If the current pivot is not one, we must divide the row
			if(base.m[pivot][pivot] != 1.0f) {
				// If the divideBy number would be zero, we must first make this row properly divisible
				if(base.m[pivot][pivot] == 0.0f) {
					for(int ar = pivot + 1; ar < 4; ++ar) {
						if(base.m[ar][pivot] != 0.0f) {
							base.m[pivot][0] += base.m[ar][0];
							base.m[pivot][1] += base.m[ar][1];
							base.m[pivot][2] += base.m[ar][2];
							base.m[pivot][3] += base.m[ar][3];
							pOut->m[pivot][0] += pOut->m[ar][0];
							pOut->m[pivot][1] += pOut->m[ar][1];
							pOut->m[pivot][2] += pOut->m[ar][2];
							pOut->m[pivot][3] += pOut->m[ar][3];
							ar = 4;
						}
					}
				}
				// Divide the row
				float divideBy = base.m[pivot][pivot];
				base.m[pivot][0] /= divideBy;
				base.m[pivot][1] /= divideBy;
				base.m[pivot][2] /= divideBy;
				base.m[pivot][3] /= divideBy;
				pOut->m[pivot][0] /= divideBy;
				pOut->m[pivot][1] /= divideBy;
				pOut->m[pivot][2] /= divideBy;
				pOut->m[pivot][3] /= divideBy;
			}

			// Perform necessary row operations to make the current diagonal the leading row value
			for(int r = 0; r < 4; ++r) {
				if(r != pivot && base.m[r][pivot] != 0.0f) {
					// Subtract the current diagonal's row from this row such that this value is equal to zero
					float times = base.m[r][pivot];
					base.m[r][0] -= times * base.m[pivot][0];
					base.m[r][1] -= times * base.m[pivot][1];
					base.m[r][2] -= times * base.m[pivot][2];
					base.m[r][3] -= times * base.m[pivot][3];
					pOut->m[r][0] -= times * pOut->m[pivot][0];
					pOut->m[r][1] -= times * pOut->m[pivot][1];
					pOut->m[r][2] -= times * pOut->m[pivot][2];
					pOut->m[r][3] -= times * pOut->m[pivot][3];
				}
			}
		}

	} else {
		pOut = 0;
	}

	if(pDeterminant) {
		*pDeterminant = determinant;
	}
	return pOut;
}

/**************************************************************************************************
* Build a matrix which scales by (sx, sy, sz)
**************************************************************************************************/
Matrix* MatrixScaling(Matrix* pOut, float sx, float sy, float sz) {
	pOut->_11 = sx;		pOut->_12 = 0.0f;	pOut->_13 = 0.0f;	pOut->_14 = 0.0f;
	pOut->_21 = 0.0f;	pOut->_22 = sy;		pOut->_23 = 0.0f;	pOut->_24 = 0.0f;
	pOut->_31 = 0.0f;	pOut->_32 = 0.0f;	pOut->_33 = sz;		pOut->_34 = 0.0f;
	pOut->_41 = 0.0f;	pOut->_42 = 0.0f;	pOut->_43 = 0.0f;	pOut->_44 = 1.0f;
	return pOut;
}

/**************************************************************************************************
* Build a matrix which translates by (x, y, z)
**************************************************************************************************/
Matrix* MatrixTranslation(Matrix* pOut, float x, float y, float z) {
	pOut->_11 = 0.0f;	pOut->_12 = 0.0f;	pOut->_13 = 0.0f;	pOut->_14 = 0.0f;
	pOut->_21 = 0.0f;	pOut->_22 = 0.0f;	pOut->_23 = 0.0f;	pOut->_24 = 0.0f;
	pOut->_31 = 0.0f;	pOut->_32 = 0.0f;	pOut->_33 = 0.0f;	pOut->_34 = 0.0f;
	pOut->_41 = x;		pOut->_42 = y;		pOut->_43 = z;		pOut->_44 = 0.0f;
	return pOut;
}

/**************************************************************************************************
* Build a matrix which rotates about the X axis
**************************************************************************************************/
Matrix* MatrixRotationX(Matrix* pOut, float angle) {
	float s = sin(angle);
	float c = cos(angle);
	pOut->_11 = 1.0f;	pOut->_12 = 0.0f;	pOut->_13 = 0.0f;	pOut->_14 = 0.0f;
	pOut->_21 = 0.0f;	pOut->_22 = c;		pOut->_23 = s;		pOut->_24 = 0.0f;
	pOut->_31 = 0.0f;	pOut->_32 = -s;		pOut->_33 = c;		pOut->_34 = 0.0f;
	pOut->_41 = 0.0f;	pOut->_42 = 0.0f;	pOut->_43 = 0.0f;	pOut->_44 = 1.0f;
	return pOut;
}

/**************************************************************************************************
* Build a matrix which rotates about the Y axis
**************************************************************************************************/
Matrix* MatrixRotationY(Matrix* pOut, float angle) {
	float s = sin(angle);
	float c = cos(angle);
	pOut->_11 = c;		pOut->_12 = 0.0f;	pOut->_13 = -s;		pOut->_14 = 0.0f;
	pOut->_21 = 0.0f;	pOut->_22 = 1.0f;	pOut->_23 = 0.0f;	pOut->_24 = 0.0f;
	pOut->_31 = s;		pOut->_32 = 0.0f;	pOut->_33 = c;		pOut->_34 = 0.0f;
	pOut->_41 = 0.0f;	pOut->_42 = 0.0f;	pOut->_43 = 0.0f;	pOut->_44 = 1.0f;
	return pOut;
}

/**************************************************************************************************
* Build a matrix which rotates about the Z axis
**************************************************************************************************/
Matrix* MatrixRotationZ(Matrix* pOut, float angle) {
	float s = sin(angle);
	float c = cos(angle);
	pOut->_11 = c;		pOut->_12 = s;		pOut->_13 = 0.0f;	pOut->_14 = 0.0f;
	pOut->_21 = -s;		pOut->_22 = c;		pOut->_23 = 0.0f;	pOut->_24 = 0.0f;
	pOut->_31 = 0.0f;	pOut->_32 = 0.0f;	pOut->_33 = 0.0f;	pOut->_34 = 0.0f;
	pOut->_41 = 0.0f;	pOut->_42 = 0.0f;	pOut->_43 = 0.0f;	pOut->_44 = 1.0f;
	return pOut;
}

/**************************************************************************************************
* Build a matrix which rotates around an arbitrary axis
**************************************************************************************************/
Matrix* MatrixRotationAxis(Matrix* pOut, const Vector3* pV, float angle) {
	float s = sin(angle);
	float c = cos(angle);
	float diffC = 1 - c;
	pOut->_11 = (pV->x * pV->x) * diffC + c;
	pOut->_12 = (pV->x * pV->y) * diffC + (pV->z * s);
	pOut->_13 = (pV->x * pV->z) * diffC - (pV->y * s);
	pOut->_14 = 0.0f;
	pOut->_21 = (pV->x * pV->y) * diffC - (pV->z * s);
	pOut->_22 = (pV->y * pV->y) * diffC + c;
	pOut->_23 = (pV->y * pV->z) * diffC + (pV->x * s);
	pOut->_24 = 0.0f;
	pOut->_31 = (pV->x * pV->z) * diffC + (pV->y * s);
	pOut->_32 = (pV->y * pV->z) * diffC - (pV->x * s);
	pOut->_33 = (pV->z * pV->z) * diffC + c;
	pOut->_34 = 0.0f;
	pOut->_41 = 0.0f;	pOut->_42 = 0.0f;	pOut->_43 = 0.0f;	pOut->_44 = 1.0f;
	return pOut;
}

/**************************************************************************************************
* Build a matrix from a quaternion
**************************************************************************************************/
Matrix* MatrixRotationQuaternion(Matrix* pOut, const Quaternion* pQ) {
	pOut->_11 = 1.0f - 2.0f*pQ->y*pQ->y - 2.0f*pQ->z*pQ->z;
	pOut->_12 = 2.0f*pQ->x*pQ->y - 2.0f*pQ->w*pQ->z;
	pOut->_13 = 2.0f*pQ->x*pQ->z + 2.0f*pQ->w*pQ->y;
	pOut->_14 = 0.0f;
	pOut->_21 = 2.0f*pQ->x*pQ->y + 2.0f*pQ->w*pQ->z;
	pOut->_22 = 1.0f - 2.0f*pQ->x*pQ->x - 2.0f*pQ->z*pQ->z;
	pOut->_23 = 2.0f*pQ->y*pQ->z - 2.0f*pQ->w*pQ->x;
	pOut->_24 = 0.0f;
	pOut->_31 = 2.0f*pQ->x*pQ->z - 2.0f*pQ->w*pQ->y;
	pOut->_32 = 2.0f*pQ->y*pQ->z + 2.0f*pQ->w*pQ->x;
	pOut->_33 = 1.0f - 2.0f*pQ->x*pQ->x - 2.0f*pQ->y*pQ->y;
	pOut->_34 = 0.0f;
	pOut->_41 = 0.0f;
	pOut->_42 = 0.0f;
	pOut->_43 = 0.0f;
	pOut->_44 = 1.0f;
	return pOut;
}

/**************************************************************************************************
* Yaw around the Y axis, a pitch around the X axis, and a roll around the Z axis
**************************************************************************************************/
Matrix* MatrixRotationYawPitchRoll(Matrix* pOut, float yaw, float pitch, float roll) {
	// Temporary matrices for holding various rotate-bys
	Matrix tM1, tM2, tM3;
	
	// Get the yaw rotation matrix
	MatrixRotationY(&tM1, yaw);
	
	// Get the pitch rotation matrix
	MatrixRotationX(&tM2, pitch);

	// Find the product of the two
	MatrixMultiply(&tM3, &tM1, &tM2);

	// Find the roll
	MatrixRotationZ(&tM1, roll);

	// Find the cumulative and set to pOut
	MatrixMultiply(pOut, &tM3, &tM1);

	// Return final matrix
	return pOut;
}

/**************************************************************************************************
* Build transformation matrix with null arguments being treated as identity. 
* Mout = Msc-1 * Msr-1 * Ms * Msr * Msc * Mrc-1 * Mr * Mrc * Mt
**************************************************************************************************/
Matrix* MatrixTransformation(Matrix* pOut, const Vector3* pScalingCenter, 
	const Quaternion* pScalingRotation, const Vector3* pScaling, const Vector3* pRotationCenter, 
	const Quaternion* pRotation, const Vector3* pTranslation) {
	// Build the various matrices
	Matrix mSC;		
	if(pScalingCenter)	MatrixTranslation(&mSC, pScalingCenter->x, pScalingCenter->y, pScalingCenter->z);
	else				MatrixIdentity(&mSC);
	
	Matrix mSCI;	
	MatrixInverse(&mSCI, 0, &mSC);
	
	Matrix mSR;
	if(pScalingRotation)	MatrixRotationQuaternion(&mSR, pScalingRotation);
	else					MatrixIdentity(&mSR);
	
	Matrix mSRI;	MatrixInverse(&mSRI, 0, &mSR);
	
	Matrix mS;		
	if(pScaling)	MatrixScaling(&mS, pScaling->x, pScaling->y, pScaling->z);
	else			MatrixIdentity(&mS);
	
	Matrix mRC;		
	if(pRotationCenter)	MatrixTranslation(&mRC, pRotationCenter->x, pRotationCenter->y, pRotationCenter->z);
	else				MatrixIdentity(&mRC);
	
	Matrix mRCI;	MatrixInverse(&mRCI, 0, &mRC);
	
	Matrix mR;
	if(pRotation)	MatrixRotationQuaternion(&mR, pRotation);
	else			MatrixIdentity(&mR);
	
	Matrix mT;		
	if(pTranslation)	MatrixTranslation(&mT, pTranslation->x, pTranslation->y, pTranslation->z);
	else				MatrixIdentity(&mT);
	
	Matrix m1, m2;
	MatrixMultiply(&m1, &mSCI, &mSRI);
	MatrixMultiply(&m2, &m1, &mS);
	MatrixMultiply(&m1, &m2, &mSR);
	MatrixMultiply(&m2, &m1, &mSC);
	MatrixMultiply(&m1, &m2, &mRCI);
	MatrixMultiply(&m2, &m1, &mR);
	MatrixMultiply(&m1, &m2, &mRC);
	MatrixMultiply(pOut, &m1, &mT);
	return pOut;
}

/**************************************************************************************************
* Build 2D transformation matrix in XY plane. Null arguments treated as identity. 
* Mout = Msc-1 * Msr-1 * Ms * Msr * Msc * Mrc-1 * Mr * Mrc * Mt
**************************************************************************************************/
Matrix* MatrixTransformation2D(Matrix* pOut, const Vector2* pScalingCenter, float scalingRotation, 
	const Vector2* pScaling, const Vector2* pRotationCenter, float rotation, const Vector2* pTranslation) {
	// Build the various matrices
	Matrix mSC;		
	if(pScalingCenter)	MatrixTranslation(&mSC, pScalingCenter->x, pScalingCenter->y, 0.0f);
	else				MatrixIdentity(&mSC);
	
	Matrix mSCI;	
	MatrixInverse(&mSCI, 0, &mSC);
	
	Matrix mSR;		MatrixRotationYawPitchRoll(&mSR, scalingRotation, scalingRotation, 0.0f);
	
	Matrix mSRI;	MatrixInverse(&mSRI, 0, &mSR);
	
	Matrix mS;		
	if(pScaling)	MatrixScaling(&mS, pScaling->x, pScaling->y, 0.0f);
	else			MatrixIdentity(&mS);
	
	Matrix mRC;		
	if(pRotationCenter)	MatrixTranslation(&mRC, pRotationCenter->x, pRotationCenter->y, 0.0f);
	else				MatrixIdentity(&mRC);
	
	Matrix mRCI;	MatrixInverse(&mRCI, 0, &mRC);
	
	Matrix mR;		MatrixRotationYawPitchRoll(&mR, rotation, rotation, 0.0f);
	
	Matrix mT;		
	if(pTranslation)	MatrixTranslation(&mT, pTranslation->x, pTranslation->y, 0.0f);
	else				MatrixIdentity(&mT);
	
	Matrix m1, m2;
	MatrixMultiply(&m1, &mSCI, &mSRI);
	MatrixMultiply(&m2, &m1, &mS);
	MatrixMultiply(&m1, &m2, &mSR);
	MatrixMultiply(&m2, &m1, &mSC);
	MatrixMultiply(&m1, &m2, &mRCI);
	MatrixMultiply(&m2, &m1, &mR);
	MatrixMultiply(&m1, &m2, &mRC);
	MatrixMultiply(pOut, &m1, &mT);
	return pOut;
}

/**************************************************************************************************
* Build 2D affine transformation matrix in XY plane. Null arguments treated as identity. 
* Mout = Ms * Mrc-1 * Mr * Mrc * Mt
**************************************************************************************************/
Matrix* MatrixAffineTransformation2D(Matrix* pOut, float scaling, const Vector2* pRotationCenter, 
	float rotation, const Vector2* pTranslation) {
	// Build the various matrices
	Matrix mS;	MatrixScaling(&mS, scaling, scaling, 0.0f);
	
	Matrix mRC;		
	if(pRotationCenter)	MatrixTranslation(&mRC, pRotationCenter->x, pRotationCenter->y, 0.0f);
	else				MatrixIdentity(&mRC);
	
	Matrix mRCI;	MatrixInverse(&mRCI, 0, &mRC);
	
	Matrix mR;		MatrixRotationYawPitchRoll(&mR, rotation, rotation, 0.0f);
	
	Matrix mT;		
	if(pTranslation)	MatrixTranslation(&mT, pTranslation->x, pTranslation->y, 0.0f);
	else				MatrixIdentity(&mT);
	
	Matrix m1, m2;
	MatrixMultiply(&m1, &mS, &mRCI);
	MatrixMultiply(&m2, &m1, &mR);
	MatrixMultiply(&m1, &m2, &mRC);
	MatrixMultiply(pOut, &m1, &mT);
	return pOut;
}

/**************************************************************************************************
* Build affine transformation matrix. Null arguments treated as identity. 
* Mout = Ms * Mrc-1 * Mr * Mrc * Mt
**************************************************************************************************/
Matrix* MatrixAffineTransformation(Matrix* pOut, float scaling, const Vector3* pRotationCenter, 
	const Quaternion* pRotation, const Vector3* pTranslation) {
	// Build various matrices
	Matrix mS;
	MatrixScaling(&mS, scaling, scaling, scaling);
	
	Matrix mRC;
	if(pRotationCenter)	MatrixTranslation(&mRC, pRotationCenter->x, pRotationCenter->y, pRotationCenter->z);
	else				MatrixIdentity(&mRC);

	Matrix mRCI;
	MatrixInverse(&mRCI, 0, &mRC);

	Matrix mR;
	if(pRotation)	MatrixRotationQuaternion(&mR, pRotation);
	else			MatrixIdentity(&mR);

	Matrix mT;
	if(pTranslation)	MatrixTranslation(&mT, pTranslation->x, pTranslation->y, pTranslation->z);
	else				MatrixIdentity(&mT);

	Matrix m1, m2;
	MatrixMultiply(&m1, &mS, &mRCI);
	MatrixMultiply(&m2, &m1, &mR);
	MatrixMultiply(&m1, &m2, &mRC);
	MatrixMultiply(pOut, &m1, &mT);
	return pOut;
}

/**************************************************************************************************
* Build a lookat matrix (right-handed)
**************************************************************************************************/
Matrix* MatrixLookAtRH(Matrix* pOut, const Vector3* pEye, const Vector3* pAt, const Vector3* pUp) {
	// Construct each local axis
	Vector3 zAxis;	Vec3Normalize(&zAxis, Vec3Sub(&zAxis, pEye, pAt));
	Vector3 xAxis;	Vec3Normalize(&xAxis, Vec3Cross(&xAxis, pUp, &zAxis));
	Vector3 yAxis;	Vec3Cross(&yAxis, &zAxis, &xAxis);

	pOut->_11 = xAxis.x;	pOut->_12 = yAxis.x;	pOut->_13 = zAxis.x;	pOut->_14 = 0.0f;
	pOut->_21 = xAxis.y;	pOut->_22 = yAxis.y;	pOut->_23 = zAxis.y;	pOut->_24 = 0.0f;
	pOut->_31 = xAxis.z;	pOut->_32 = yAxis.z;	pOut->_33 = zAxis.z;	pOut->_34 = 0.0f;
	pOut->_41 = Vec3Dot(&xAxis, pEye);
	pOut->_42 = Vec3Dot(&yAxis, pEye);
	pOut->_43 = Vec3Dot(&zAxis, pEye);
	pOut->_44 = 1.0f;

	return pOut;
}

/**************************************************************************************************
* Build a lookat matrix (left-handed)
**************************************************************************************************/
Matrix* MatrixLookAtLH(Matrix* pOut, const Vector3* pEye, const Vector3* pAt, const Vector3* pUp) {
	// Construct each local axis
	Vector3 zAxis;	Vec3Normalize(&zAxis, Vec3Sub(&zAxis, pAt, pEye));
	Vector3 xAxis;	Vec3Normalize(&xAxis, Vec3Cross(&xAxis, pUp, &zAxis));
	Vector3 yAxis;	Vec3Cross(&yAxis, &zAxis, &xAxis);

	pOut->_11 = xAxis.x;	pOut->_12 = yAxis.x;	pOut->_13 = zAxis.x;	pOut->_14 = 0.0f;
	pOut->_21 = xAxis.y;	pOut->_22 = yAxis.y;	pOut->_23 = zAxis.y;	pOut->_24 = 0.0f;
	pOut->_31 = xAxis.z;	pOut->_32 = yAxis.z;	pOut->_33 = zAxis.z;	pOut->_34 = 0.0f;
	pOut->_41 = -Vec3Dot(&xAxis, pEye);
	pOut->_42 = -Vec3Dot(&yAxis, pEye);
	pOut->_43 = -Vec3Dot(&zAxis, pEye);
	pOut->_44 = 1.0f;

	return pOut;
}

/**************************************************************************************************
* Build a perspective projection matrix (right-handed)
**************************************************************************************************/
Matrix* MatrixPerspectiveRH(Matrix* pOut, float w, float h, float zn, float zf) {
	pOut->_11 = 2.0f * zn / w;
	pOut->_12 = 0.0f;
	pOut->_13 = 0.0f;
	pOut->_14 = 0.0f;
	pOut->_21 = 0.0f;
	pOut->_22 = 2.0f * zn / h;
	pOut->_23 = 0.0f;
	pOut->_24 = 0.0f;
	pOut->_31 = 0.0f;
	pOut->_32 = 0.0f;
	pOut->_33 = zf / (zn - zf);
	pOut->_34 = -1.0f;
	pOut->_41 = 0.0f;
	pOut->_42 = 0.0f;
	pOut->_43 = (zn * zf) / (zn - zf);
	pOut->_44 = 0.0f;
	return pOut;
}

/**************************************************************************************************
* Build a perspective projection matrix (left-handed)
**************************************************************************************************/
Matrix* MatrixPerspectiveLH(Matrix* pOut, float w, float h, float zn, float zf) {
	pOut->_11 = 2.0f * zn / w;
	pOut->_12 = 0.0f;
	pOut->_13 = 0.0f;
	pOut->_14 = 0.0f;
	pOut->_21 = 0.0f;
	pOut->_22 = 2.0f * zn / h;
	pOut->_23 = 0.0f;
	pOut->_24 = 0.0f;
	pOut->_31 = 0.0f;
	pOut->_32 = 0.0f;
	pOut->_33 = zf / (zf - zn);
	pOut->_34 = 1.0f;
	pOut->_41 = 0.0f;
	pOut->_42 = 0.0f;
	pOut->_43 = (zn * zf) / (zn - zf);
	pOut->_44 = 0.0f;
	return pOut;
}

/**************************************************************************************************
* Build a perspective projection matrix (right-handed)
**************************************************************************************************/
Matrix* MatrixPerspectiveFovRH(Matrix* pOut, float fovy, float aspect, float zn, float zf) {
	float yScale = atan(fovy / 2.0f);
	float xScale = yScale / aspect;

	pOut->_11 = xScale;
	pOut->_12 = 0.0f;
	pOut->_13 = 0.0f;
	pOut->_14 = 0.0f;
	pOut->_21 = 0.0f;
	pOut->_22 = yScale;
	pOut->_23 = 0.0f;
	pOut->_24 = 0.0f;
	pOut->_31 = 0.0f;
	pOut->_32 = 0.0f;
	pOut->_33 = zf / (zn - zf);
	pOut->_34 = -1.0f;
	pOut->_41 = 0.0f;
	pOut->_42 = 0.0f;
	pOut->_43 = (zn * zf) / (zn - zf);
	pOut->_44 = 0.0f;

	return pOut;
}

/**************************************************************************************************
* Build a perspective projection matrix (left-handed)
**************************************************************************************************/
Matrix* MatrixPerspectiveFovLH(Matrix* pOut, float fovy, float aspect, float zn, float zf) {
	float yScale = atan(fovy / 2.0f);
	float xScale = yScale / aspect;

	pOut->_11 = xScale;
	pOut->_12 = 0.0f;
	pOut->_13 = 0.0f;
	pOut->_14 = 0.0f;
	pOut->_21 = 0.0f;
	pOut->_22 = yScale;
	pOut->_23 = 0.0f;
	pOut->_24 = 0.0f;
	pOut->_31 = 0.0f;
	pOut->_32 = 0.0f;
	pOut->_33 = zf / (zf - zn);
	pOut->_34 = 1.0f;
	pOut->_41 = 0.0f;
	pOut->_42 = 0.0f;
	pOut->_43 = -(zn * zf) / (zf - zn);
	pOut->_44 = 0.0f;

	return pOut;
}

/**************************************************************************************************
* Build a matrix which flattens geometry into a plane, as if casting a shadow from a light
**************************************************************************************************/
Matrix* MatrixShadow(Matrix* pOut, const Vector4* pLight, const Plane* pPlane) {
	// Normalize the plane
	Plane p;
	PlaneNormalize(&p, pPlane);

	// Calculate the dot product between the plane and the vector
	float d = -(pLight->x*pPlane->a + pLight->y*pPlane->b + pLight->z*pPlane->c + pLight->w*pPlane->d);

	// Construct the matrix
	pOut->_11 = p.a * pLight->x + d;
	pOut->_12 = p.a * pLight->y;
	pOut->_13 = p.a * pLight->z;
	pOut->_14 = p.a * pLight->w;
	pOut->_21 = p.b * pLight->x;
	pOut->_22 = p.b * pLight->y + d;
	pOut->_23 = p.b * pLight->z;
	pOut->_24 = p.b * pLight->w;
	pOut->_31 = p.c * pLight->x;
	pOut->_32 = p.c * pLight->y;
	pOut->_33 = p.c * pLight->z + d;
	pOut->_34 = p.c * pLight->w;
	pOut->_41 = p.d * pLight->x;
	pOut->_42 = p.d * pLight->y;
	pOut->_43 = p.d * pLight->z;
	pOut->_44 = p.d * pLight->w + d;

	return pOut;
}

/**************************************************************************************************
* Build a matrix which reflects the coordinate system about a plane
**************************************************************************************************/
Matrix* MatrixReflect(Matrix* pOut, const Plane* pPlane) {
	// Normalize the plane
	Plane p;
	PlaneNormalize(&p, pPlane);

	// Construct the matrix
	pOut->_11 = -2.0f * p.a * p.a + 1.0f;
	pOut->_12 = -2.0f * p.b * p.a;
	pOut->_13 = -2.0f * p.c * p.a;
	pOut->_14 = 0.0f;
	pOut->_21 = -2.0f * p.a * p.b;
	pOut->_22 = -2.0f * p.b * p.b + 1.0f;
	pOut->_23 = -2.0f * p.c * p.b;
	pOut->_24 = 0.0f;
	pOut->_31 = -2.0f * p.a * p.c;
	pOut->_32 = -2.0f * p.b * p.c;
	pOut->_33 = -2.0f * p.c * p.c + 1.0f;
	pOut->_34 = 0.0f;
	pOut->_41 = -2.0f * p.a * p.d;
	pOut->_42 = -2.0f * p.b * p.d;
	pOut->_43 = -2.0f * p.c * p.d;
	pOut->_44 = 1.0f;
	return pOut;
}

/**************************************************************************************************
* Creates a tensor product of given two 3D vectors
**************************************************************************************************/
Matrix* MatrixTensorProduct(Matrix* pOut, const Vector3* pV1, const Vector3* pV2) {
	pOut->_11 = pV1->x * pV2->x;
	pOut->_12 = pV1->x * pV2->y;
	pOut->_13 = pV1->x * pV2->z;
	pOut->_14 = 0.0f;
	pOut->_21 = pV1->y * pV2->x;
	pOut->_22 = pV1->y * pV2->y;
	pOut->_23 = pV1->y * pV2->z;
	pOut->_24 = 0.0f;
	pOut->_31 = pV1->z * pV2->x;
	pOut->_32 = pV1->z * pV2->y;
	pOut->_33 = pV1->z * pV2->z;
	pOut->_34 = 0.0f;
	pOut->_41 = 0.0f;
	pOut->_42 = 0.0f;
	pOut->_43 = 0.0f;
	pOut->_44 = 1.0f;
	return pOut;
}